// juilus speach recognition node for ROS
// by David Martin, 2011
// 
// Code & comments adapted from julius-simple example

#include <sstream>

#include "ros/ros.h"

#include <julius/juliuslib.h>
#include "julius/conf.h"
#include "julius/Sentence.h"

std::ostringstream ossBuffer;
ros::Publisher speech_pub;

// Callback to be called when start waiting speech input. 
static void status_recready(Recog *recog, void *dummy) { ROS_DEBUG("<<< please speak >>>"); }

// Callback to be called when speech input is triggered.
static void status_recstart(Recog *recog, void *dummy) { ROS_DEBUG("<<< listening >>>"); }

// Callback to output final recognition result. (called just after recognition of an input ends)
static void output_result(Recog *recog, void *dummy) {
  // All recognition results are stored at each recognition process instance
  for (RecogProcess *r = recog->process_list; r; r=r->next) {
    if (!r->live) continue; // skip the process if the process is not alive

    /* result are in r->result.  See recog.h for details */

    // Check result status
    if (r->result.status < 0) { // No results obtained
      // Output message according to the status code
      switch(r->result.status) {
        case J_RESULT_STATUS_REJECT_POWER:
	        ROS_INFO("<input rejected by power>");
	        break;
        case J_RESULT_STATUS_TERMINATE:
	        ROS_INFO("<input teminated by request>");
	        break;
        case J_RESULT_STATUS_ONLY_SILENCE:
	        ROS_INFO("<input rejected by decoder (silence input result)>");
	        break;
        case J_RESULT_STATUS_REJECT_GMM:
	        ROS_INFO("<input rejected by GMM>");
	        break;
        case J_RESULT_STATUS_REJECT_SHORT:
	        ROS_INFO("<input rejected by short input>");
	        break;
        case J_RESULT_STATUS_FAIL:
	        ROS_INFO("<search failed>");
	        break;
      }
      continue; // Continue to next process instance
    }

    // Output results for all the obtained sentences
    WORD_INFO *winfo = r->lm->winfo;

    //ROS_INFO("Result status %d", r->result.status);
    //ROS_INFO("Result sentnum %d", r->result.sentnum);
    for (int n = 0; n < r->result.sentnum; n++) { /* for all sentences */
      Sentence *s = &(r->result.sent[n]);
      WORD_ID *seq = s->word;
      int seqnum = s->word_num;
      julius::Sentence msg;

      // Output word sequence
      for (int i = 0; i < seqnum; i++) {
        std::string word(winfo->woutput[seq[i]]);
        msg.text += word + ' ';
        msg.words.push_back(word);
        ossBuffer << ' ' << word;
      }
      ROS_INFO_STREAM("sentence" << n + 1 << ':' << ossBuffer.str());
      ossBuffer.str("");
      
      // Output LM entry sequence
      for (int i = 0; i < seqnum; i++)
        ossBuffer << ' ' << winfo->wname[seq[i]];
      ROS_DEBUG_STREAM("wseq" << n + 1 << ':' << ossBuffer.str());
      ossBuffer.str("");
      
      // Output confidence scores
      for (int i = 0; i < seqnum; i++) {
        msg.word_scores.push_back(s->confidence[i]);
        ossBuffer << ' ' << s->confidence[i]; //printf(" %5.3f", s->confidence[i]);
      }
      ROS_DEBUG_STREAM("cmscore" << n + 1 << ':' << ossBuffer.str());
      ossBuffer.str("");
      
      /* AM and LM scores */
      msg.score    = s->score;
      msg.score_lm = s->score_lm;
      msg.score_am = s->score_am;
      ROS_DEBUG("score%d: %f", n+1, s->score);
      if (r->lmtype == LM_PROB) // If this process uses N-gram
	      ROS_DEBUG("score%d: AM: %f  LM: %f", n+1, s->score_am, s->score_lm);
      
      // Grammar
      msg.grammar_id = s->gram_id;
      if (r->lmtype == LM_DFA) { /* if this process uses DFA grammar */
	      /* output which grammar the hypothesis belongs to
	         when using multiple grammars */
	      if (multigram_get_all_num(r->lm) > 1) {
	        ROS_DEBUG("grammar%d: %d", n+1, s->gram_id);
	      }
      }
      
      /* output alignment result if exist */
      for (SentenceAlign *align = s->align; align; align = align->next) {
	      ROS_INFO("=== begin forced alignment ===");
	      switch(align->unittype) {
	        case PER_WORD:
	          ROS_INFO("-- word alignment --"); break;
	        case PER_PHONEME:
	          ROS_INFO("-- phoneme alignment --"); break;
	        case PER_STATE:
	          ROS_INFO("-- state alignment --"); break;
	      }
	      ROS_INFO(" id: from  to    n_score    unit");
	      ROS_INFO(" ----------------------------------------");
	      for(int i = 0; i < align->num; i++) {
	        printf("[%4d %4d]  %f  ", align->begin_frame[i], align->end_frame[i], align->avgscore[i]);
	        HMM_Logical *p;
	        switch(align->unittype) {
	          case PER_WORD:
	            printf("%s\t[%s]\n", winfo->wname[align->w[i]], winfo->woutput[align->w[i]]);
	            break;
	          case PER_PHONEME:
	            p = align->ph[i];
	            if (p->is_pseudo) {
	              printf("{%s}\n", p->name);
	            } else if (strmatch(p->name, p->body.defined->name)) {
	              printf("%s\n", p->name);
	            } else {
	              printf("%s[%s]\n", p->name, p->body.defined->name);
	            }
	            break;
	          case PER_STATE:
	            p = align->ph[i];
	            if (p->is_pseudo) {
	              printf("{%s}", p->name);
	            } else if (strmatch(p->name, p->body.defined->name)) {
	              printf("%s", p->name);
	            } else {
	              printf("%s[%s]", p->name, p->body.defined->name);
	            }
	            if (r->am->hmminfo->multipath) {
	              if (align->is_iwsp[i]) {
		              printf(" #%d (sp)\n", align->loc[i]);
	              } else {
		              printf(" #%d\n", align->loc[i]);
	              }
	            } else {
	              printf(" #%d\n", align->loc[i]);
	            }
	            break;
	        }
	      }
	
	      ROS_INFO("re-computed AM score: %f", align->allscore);
	      ROS_INFO("=== end forced alignment ===");
      }
      speech_pub.publish(msg);
    }
  }

  /* flush output buffer */
  fflush(stdout);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "julius_listener");
  ros::NodeHandle nh;
  speech_pub = nh.advertise<julius::Sentence>("recognized_speech", 1000);

  Jconf *jconf; // Configuration parameter holder
  Recog *recog; // Recognition instance
  
  // Load Config
  //jconf = j_config_load_args_new(argc, argv);
  jconf = j_config_load_file_new(JULIUS_JCONF_PATH);
  if (jconf == NULL) { // If config is still invalid, return now.
    ROS_ERROR("No valid jconf file.");
    return -1; // ERROR
  }
  
  // Create recognition instance from the jconf
  /* This loads models, sets up final parameters, abuilds the lexicon
   * and sets up the work area for recognition */
  recog = j_create_instance_from_jconf(jconf);
  if (recog == NULL) {
    ROS_ERROR("Error in startup");
    return -1;
  }

  // Register Callbacks
  callback_add(recog, CALLBACK_EVENT_SPEECH_READY, status_recready, NULL);
  callback_add(recog, CALLBACK_EVENT_SPEECH_START, status_recstart, NULL);
  callback_add(recog, CALLBACK_RESULT, output_result, NULL);

  // Initialize audio input device
  //   The ad-in thread starts at this time for microphone 
  if (j_adin_init(recog) == FALSE)
    return -1; // ERROR

  j_recog_info(recog); // Output system information to log

  // Open input stream and begin recognition
  /* raw speech input (microphone etc.) */
  switch(j_open_stream(recog, NULL)) {
    case 0: // Succeeded
      break;
    case -1: // Error
      ROS_ERROR("Error in input stream");
      return -1;
    case -2: // End of recognition process
      ROS_ERROR("Failed to begin input stream");
      return -1;
  }
  
  // Begin recognition loop
  int ret = j_recognize_stream(recog);
  if (ret == -1) return -1;	// Error
  
  // End & Clean Up
  j_close_stream(recog); // Terminates recognition and exit j_recognize_stream()
  j_recog_free(recog);
  
  return 0;
}
